#include <iostream>
#include <thread>
#include <string>
#include <time.h>

#include <pcl/io/pcd_io.h> //pcl的pcd格式io文件
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>               //使用ndt配准头文件
#include <pcl/filters/approximate_voxel_grid.h> //使用这个滤波

#include <pcl/visualization/pcl_visualizer.h> //可视化头文件,基于vtk

using namespace std::chrono_literals;

int main(int argc, char **argv)
{
    std::string target_cloud_path = "../data/room_scan1.pcd", input_cloud_path = "../data/room_scan2.pcd";

    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>); //目标点云
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(target_cloud_path, *target_cloud) == -1)
    {
        PCL_ERROR("could not read file %s\n", target_cloud_path);
        return -1;
    }
    std::cout << "Loaded " << target_cloud->size() << " data points from" << target_cloud_path << std::endl;

    //读取pcd文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>); //原点云
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(input_cloud_path, *input_cloud) == -1)
    {
        PCL_ERROR("could not read file %s\n", input_cloud_path);
        return -1;
    }
    std::cout << "Loaded " << input_cloud->size() << " data points from " << input_cloud_path << std::endl;

    //将源点云过滤到原始尺寸的10%,提高匹配速度,只对点云进行滤波,目标点云不用滤波
    // NDT算法中在目标点云对应的体素网格数据结构的统计计算中不用单个点,而用包含在每个体素单元格中的点统计数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); //存滤波后的点云
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;                      //创建滤波用的对象
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);                                    //设置体素单元格的长宽高
    approximate_voxel_filter.setInputCloud(input_cloud);
    approximate_voxel_filter.filter(*filtered_cloud); //滤波

    std::cout << "Filtered cloud contains " << filtered_cloud->size() << " data points from " << input_cloud_path << std ::endl;

    //利用PCL库的NDT配准两帧点云
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; //设置配准前后的点云类型
    ndt.setTransformationEpsilon(0.001);                                 //设置容差
    ndt.setStepSize(0.05);                                               //设置牛顿迭代法的最大步长
    ndt.setResolution(1.0);                                              //设置网格化立方体边长
    ndt.setMaximumIterations(50);                                        //设置最大迭代次数
    ndt.setInputSource(filtered_cloud);                                  //将滤波后的原点云作为配准原点云,根据pcl库的warning,setInputCloud现已被setInputSource取代
    ndt.setInputTarget(target_cloud);

    Eigen::AngleAxisf init_rotation(0.693, Eigen::Vector3f::UnitZ()); //旋转向量(绕z轴旋转0.693 rad)
    Eigen::Translation3f init_translation(1.79387, 0, 0);

    Eigen::Matrix4f init_guss = (init_translation * init_rotation).matrix();

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    time_t start_time;
    time(&start_time);
    std::cout << "Start to align! waiting!" << std::endl;
    ndt.align(*output_cloud, init_guss); //根据初始猜测进行配准,结果存到output_cloud
    time_t end_time;
    time(&end_time);
    if (ndt.hasConverged())
    {
        std::cout << "aligned successfully! time use: " << difftime(end_time, start_time) << "s" << std::endl;
        std::cout << "Normal Distributions Transform has converged" << std::endl;
    }
    else
    {
        std::cout << "Aligned unsuccessfully!" << endl;
        return -1;
    }

    //欧几里得适合度的分FitnessScore,输出从输出点云到目标点云中最近点的距离的平方.分数月大,准确率越低
    std::cout << "Score(分数越大,配准效果越差): " << ndt.getFitnessScore() << std::endl;

    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation()); //使用未过滤的原点云进行变换

    pcl::visualization::PCLVisualizer::Ptr pclviewer_ptr(new pcl::visualization::PCLVisualizer);
    pclviewer_ptr->setBackgroundColor(0, 0, 0);

    //对目标点云可视化(红色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    pclviewer_ptr->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
    pclviewer_ptr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); //设置点大小为1

    //对转换后的点云可视化(蓝色)
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(output_cloud, 0, 0, 255);
    pclviewer_ptr->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
    pclviewer_ptr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud"); //设置点大小为1

    //设置显示信息
    pclviewer_ptr->addCoordinateSystem(1.0, "global"); //显示坐标轴
    pclviewer_ptr->initCameraParameters();

    while (!pclviewer_ptr->wasStopped())
    {
        pclviewer_ptr->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }

    return 0;
}